/*******************************************************************************
*
*   FILE NAME:
*       Matrix.cpp - Matrix class implementation
*
*   DESCRIPTION:
*       Defines a 4x4 matrix.
*
*******************************************************************************/
#include "Matrix.h"

#include <math.h>
#include <string.h>

const Matrix Matrix::Identity = Matrix( 1.0f, 0.0f, 0.0f, 0.0f,
                                        0.0f, 1.0f, 0.0f, 0.0f,
                                        0.0f, 0.0f, 1.0f, 0.0f,
                                        0.0f, 0.0f, 0.0f, 1.0f );

Matrix::Matrix( void )
    : M11( 1.0f ), M12( 0.0f ), M13( 0.0f ), M14( 0.0f ),
      M21( 0.0f ), M22( 1.0f ), M23( 0.0f ), M24( 0.0f ),
      M31( 0.0f ), M32( 0.0f ), M33( 1.0f ), M34( 0.0f ),
      M41( 0.0f ), M42( 0.0f ), M43( 0.0f ), M44( 1.0f )
{
}

Matrix::Matrix( float32 m11, float32 m12, float32 m13, float32 m14, float32 m21, float32 m22, float32 m23, float32 m24, float32 m31, float32 m32, float32 m33, float32 m34, float32 m41, float32 m42, float32 m43, float32 m44 )
    : M11( m11 ), M12( m12 ), M13( m13 ), M14( m14 ),
      M21( m21 ), M22( m22 ), M23( m23 ), M24( m24 ),
      M31( m31 ), M32( m32 ), M33( m33 ), M34( m34 ),
      M41( m41 ), M42( m42 ), M43( m43 ), M44( m44 )
{
}

Matrix::Matrix( const float32* matrix_data )
{
    memcpy( &M11, matrix_data, sizeof( Matrix ) );
}

Matrix::Matrix( const Matrix& other )
{
    memcpy( &M11, &other.M11, sizeof( Matrix ) );
}

Matrix::~Matrix( void )
{
}

Matrix& Matrix::operator=( const Matrix& other )
{
    memcpy( &M11, &other.M11, sizeof( Matrix ) );
    return *this;
}

boolean operator==( const Matrix& mat1, const Matrix& mat2 )
{
    for( uint8 i = 0; i < 16; ++i )
    {
        if( ( &mat1.M11 )[ i ] != ( &mat2.M11 )[ i ] )
        {
            return FALSE;
        }
    }
    return TRUE;
}

boolean operator!=( const Matrix& mat1, const Matrix& mat2 )
{
    for( uint8 i = 0; i < 16; ++i )
    {
        if( ( &mat1.M11 )[ i ] != ( &mat2.M11 )[ i ] )
        {
            return TRUE;
        }
    }
    return FALSE;
}

Matrix operator+( const Matrix& mat1, const Matrix& mat2 )
{
    return Matrix::Add( mat1, mat2 );
}

Matrix operator-( const Matrix& mat1, const Matrix& mat2 )
{
    return Matrix::Subtract( mat1, mat2 );
}

Matrix operator*( const Matrix& mat1, const Matrix& mat2 )
{
    return Matrix::Multiply( mat1, mat2 );
}

Matrix operator*( const Matrix& mat, float32 scalar )
{
    return Matrix::Multiply( mat, scalar );
}

Matrix operator*( float32 scalar, const Matrix& mat )
{
    return Matrix::Multiply( mat, scalar );
}

Matrix operator/( const Matrix& mat1, const Matrix& mat2 )
{
    return Matrix::Divide( mat1, mat2 );
}

Matrix operator/( const Matrix& mat, float32 divider )
{
    return Matrix::Divide( mat, divider );
}

Matrix Matrix::Add( const Matrix& mat1, const Matrix& mat2 )
{
    Matrix ret;
    Add( mat1, mat2, ret );
    return ret;
}

Matrix Matrix::Divide( const Matrix& mat1, const Matrix& mat2 )
{
    Matrix ret;
    Divide( mat1, mat2, ret );
    return ret;
}

Matrix Matrix::Divide( const Matrix& mat, float32 divider )
{
    Matrix ret;
    Divide( mat, divider, ret );
    return ret;
}

Matrix Matrix::Invert( const Matrix& mat )
{
    Matrix ret;
    Invert( mat, ret );
    return ret;
}

Matrix Matrix::Multiply( const Matrix& mat1, const Matrix& mat2 )
{
    Matrix ret;
    Multiply( mat1, mat2, ret );
    return ret;
}

Matrix Matrix::Multiply( const Matrix& mat, float32 scalar )
{
    Matrix ret;
    Multiply( mat, scalar, ret );
    return ret;
}

Matrix Matrix::Subtract( const Matrix& mat1, const Matrix& mat2 )
{
    Matrix ret;
    Subtract( mat1, mat2, ret );
    return ret;
}

Matrix Matrix::Transpose( const Matrix& mat )
{
    Matrix ret;
    Transpose( mat, ret );
    return ret;
}

void Matrix::Add( const Matrix& mat1, const Matrix& mat2, Matrix& result )
{
    for( uint8 i = 0; i < 16; ++i )
    {
        ( &result.M11 )[ i ] = ( &mat1.M11 )[ i ] + ( &mat2.M11 )[ i ];
    }
}

void Matrix::Divide( const Matrix& mat1, const Matrix& mat2, Matrix& result )
{
    for( uint8 i = 0; i < 16; ++i )
    {
        ( &result.M11 )[ i ] = ( &mat1.M11 )[ i ] / ( &mat2.M11 )[ i ];
    }
}

void Matrix::Divide( const Matrix& mat, float32 divider, Matrix& result )
{
    for( uint8 i = 0; i < 16; ++i )
    {
        ( &result.M11 )[ i ] = ( &mat.M11 )[ i ] / divider;
    }
}

void Matrix::Invert( const Matrix& mat, Matrix& result )
{
    //TODO
}

void Matrix::Multiply( const Matrix& mat1, const Matrix& mat2, Matrix& result )
{
    Matrix tmp;

    tmp.M11 = ( mat1.M11 * mat2.M11 ) + ( mat1.M12 * mat2.M21 ) + ( mat1.M13 * mat2.M31 ) + ( mat1.M14 * mat2.M41 );
    tmp.M12 = ( mat1.M11 * mat2.M12 ) + ( mat1.M12 * mat2.M22 ) + ( mat1.M13 * mat2.M32 ) + ( mat1.M14 * mat2.M42 );
    tmp.M13 = ( mat1.M11 * mat2.M13 ) + ( mat1.M12 * mat2.M23 ) + ( mat1.M13 * mat2.M33 ) + ( mat1.M14 * mat2.M43 );
    tmp.M14 = ( mat1.M11 * mat2.M14 ) + ( mat1.M12 * mat2.M24 ) + ( mat1.M13 * mat2.M34 ) + ( mat1.M14 * mat2.M44 );
    
    tmp.M11 = ( mat1.M21 * mat2.M11 ) + ( mat1.M22 * mat2.M21 ) + ( mat1.M23 * mat2.M31 ) + ( mat1.M24 * mat2.M41 );
    tmp.M12 = ( mat1.M21 * mat2.M12 ) + ( mat1.M22 * mat2.M22 ) + ( mat1.M23 * mat2.M32 ) + ( mat1.M24 * mat2.M42 );
    tmp.M13 = ( mat1.M21 * mat2.M13 ) + ( mat1.M22 * mat2.M23 ) + ( mat1.M23 * mat2.M33 ) + ( mat1.M24 * mat2.M43 );
    tmp.M14 = ( mat1.M21 * mat2.M14 ) + ( mat1.M22 * mat2.M24 ) + ( mat1.M23 * mat2.M34 ) + ( mat1.M24 * mat2.M44 );
    
    tmp.M11 = ( mat1.M31 * mat2.M11 ) + ( mat1.M32 * mat2.M21 ) + ( mat1.M33 * mat2.M31 ) + ( mat1.M34 * mat2.M41 );
    tmp.M12 = ( mat1.M31 * mat2.M12 ) + ( mat1.M32 * mat2.M22 ) + ( mat1.M33 * mat2.M32 ) + ( mat1.M34 * mat2.M42 );
    tmp.M13 = ( mat1.M31 * mat2.M13 ) + ( mat1.M32 * mat2.M23 ) + ( mat1.M33 * mat2.M33 ) + ( mat1.M34 * mat2.M43 );
    tmp.M14 = ( mat1.M31 * mat2.M14 ) + ( mat1.M32 * mat2.M24 ) + ( mat1.M33 * mat2.M34 ) + ( mat1.M34 * mat2.M44 );
    
    tmp.M11 = ( mat1.M41 * mat2.M11 ) + ( mat1.M42 * mat2.M21 ) + ( mat1.M43 * mat2.M31 ) + ( mat1.M44 * mat2.M41 );
    tmp.M12 = ( mat1.M41 * mat2.M12 ) + ( mat1.M42 * mat2.M22 ) + ( mat1.M43 * mat2.M32 ) + ( mat1.M44 * mat2.M42 );
    tmp.M13 = ( mat1.M41 * mat2.M13 ) + ( mat1.M42 * mat2.M23 ) + ( mat1.M43 * mat2.M33 ) + ( mat1.M44 * mat2.M43 );
    tmp.M14 = ( mat1.M41 * mat2.M14 ) + ( mat1.M42 * mat2.M24 ) + ( mat1.M43 * mat2.M34 ) + ( mat1.M44 * mat2.M44 );

    memcpy( &result, &tmp, sizeof( Matrix ) );
}

void Matrix::Multiply( const Matrix& mat, float32 scalar, Matrix& result )
{
    for( uint8 i = 0; i < 16; ++i )
    {
        ( &result.M11 )[ i ] = ( &mat.M11 )[ i ] * scalar;
    }
}

void Matrix::Subtract( const Matrix& mat1, const Matrix& mat2, Matrix& result )
{
    for( uint8 i = 0; i < 16; ++i )
    {
        ( &result.M11 )[ i ] = ( &mat1.M11 )[ i ] - ( &mat2.M11 )[ i ];
    }
}

void Matrix::Transpose( const Matrix& mat, Matrix& result )
{
    Matrix tmp( mat.M11, mat.M21, mat.M31, mat.M41,
                mat.M12, mat.M22, mat.M32, mat.M42,
                mat.M13, mat.M23, mat.M33, mat.M43,
                mat.M14, mat.M24, mat.M34, mat.M44 );

    memcpy( &result, &tmp, sizeof( Matrix ) );
}

Matrix Matrix::CreateLookAt( const Vector3& camera, const Vector3& target, const Vector3& up )
{
    Matrix ret;
    CreateLookAt( camera, target, up, ret );
    return ret;
}

Matrix Matrix::CreatePerspectiveFieldOfView( float32 field_of_view, float32 aspect_ratio, float32 near_plane, float32 far_plane )
{
    Matrix ret;
    CreatePerspectiveFieldOfView( field_of_view, aspect_ratio, near_plane, far_plane, ret );
    return ret;
}

void Matrix::CreateLookAt( const Vector3& camera, const Vector3& target, const Vector3& up, Matrix& result )
{
    Vector3 z_axis = Vector3::Normalize( camera - target );
    Vector3 x_axis = Vector3::Normalize( Vector3::Cross( up, z_axis ) );
    Vector3 y_axis = Vector3::Cross( z_axis, x_axis );

    result.M11 = x_axis.X;
    result.M12 = y_axis.X;
    result.M13 = z_axis.X;
    result.M14 = 0.0f;
    
    result.M21 = x_axis.Y;
    result.M22 = y_axis.Y;
    result.M23 = z_axis.Y;
    result.M24 = 0.0f;

    result.M31 = x_axis.Z;
    result.M32 = y_axis.Z;
    result.M33 = z_axis.Z;
    result.M34 = 0.0f;

    result.M41 = Vector3::Dot( x_axis, target );
    result.M42 = Vector3::Dot( y_axis, target );
    result.M43 = Vector3::Dot( z_axis, target );
    result.M44 = 1.0f;
}

void Matrix::CreatePerspectiveFieldOfView( float32 field_of_view, float32 aspect_ratio, float32 near_plane, float32 far_plane, Matrix& result )
{
    float32 y_scale = 1.0f / tan( field_of_view / 2.0f );
    float32 x_scale = y_scale / aspect_ratio;
    
    result.M11 = x_scale;
    result.M12 = 0.0f;
    result.M13 = 0.0f;
    result.M14 = 0.0f;
    
    result.M21 = 0.0f;
    result.M22 = y_scale;
    result.M23 = 0.0f;
    result.M24 = 0.0f;

    result.M31 = 0.0f;
    result.M32 = 0.0f;
    result.M33 = far_plane / ( near_plane - far_plane );
    result.M34 = -1.0f;

    result.M41 = 0.0f;
    result.M42 = 0.0f;
    result.M43 = ( near_plane * far_plane ) / ( near_plane - far_plane );
    result.M44 = 0.0f;
}
